#include <mc_controller/controller_base.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

ControllerBase::ControllerBase():baseFrameId_("base_link"){
    speedPub_ = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    pathLocalPub_ = nh_.advertise<nav_msgs::Path>("/local_path", 10);
    poseLookaheadPub_ = nh_.advertise<geometry_msgs::PoseStamped>("/lookahead_pose", 10);

    localOriginPose_.pose.orientation.w = 1.0;
}

ControllerBase::~ControllerBase(){

}

/*************************************************
* 位姿转换
*************************************************/
// 获取TF坐标变换
bool ControllerBase::getTFPose(tf2_ros::Buffer* tf_buffer, geometry_msgs::PoseStamped& global_pose, string father_frame, string child_frame, ros::Time stamp){
    tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
    geometry_msgs::PoseStamped robot_pose;
    tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
    robot_pose.header.frame_id = child_frame;
    robot_pose.header.stamp = ros::Time();
    // get the global pose of the robot
    try
    {
        // use current time if possible (makes sure it's not in the future)
        if (tf_buffer->canTransform(father_frame, child_frame, stamp))
        {
            geometry_msgs::TransformStamped transform = tf_buffer->lookupTransform(father_frame, child_frame, stamp);
            tf2::doTransform(robot_pose, global_pose, transform);
        }
        // use the latest otherwise
        else
        {
            tf_buffer->transform(robot_pose, global_pose, father_frame);
        }
    }
    catch (tf2::LookupException& ex)
    {
        ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
        return false;
    }
    catch (tf2::ConnectivityException& ex)
    {
        ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
        return false;
    }
    catch (tf2::ExtrapolationException& ex)
    {
        ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
        return false;
    }
    // // check global_pose timeout
    // ros::Time current_time = ros::Time::now();
    // if(current_time.toSec() - global_pose.header.stamp.toSec() > __transform_tolerance)
    // {
    //     ROS_WARN_THROTTLE(1.0,
    //                     "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
    //                     current_time.toSec(), global_pose.header.stamp.toSec(), __transform_tolerance);
    //     return false;
    // }

    return true;        
}

// 获取机器人当前位姿
bool ControllerBase::getRobotPose(tf2_ros::Buffer* tf_buffer, geometry_msgs::PoseStamped& global_pose){
    // 尝试3次
    for(int i=0; i<3; i++){
        if(getTFPose(tf_buffer, global_pose, "map", baseFrameId_, ros::Time::now())){
            return true;
        }
        ros::Duration(0.05).sleep();
    }
    return false;
}

// 全局位姿转局部
geometry_msgs::PoseStamped ControllerBase::convertGlobalPoseToLocal(geometry_msgs::PoseStamped global_pose, 
    geometry_msgs::PoseStamped robot_pose){
    geometry_msgs::PoseStamped local_pose;

    // 方式一
    tf2::Transform global_pose_tf;
    tf2::fromMsg(global_pose.pose, global_pose_tf);
    tf2::Transform robot_pose_tf;
    tf2::fromMsg(robot_pose.pose, robot_pose_tf);

    // 计算相对变换（全局变换到局部变换）
    tf2::Transform local_pose_tf = robot_pose_tf.inverse() * global_pose_tf;
    tf2::toMsg(local_pose_tf, local_pose.pose);
    
    // 方式二
    // double yaw = tf2::getYaw(robot_pose.pose.orientation);
    // double delta_x = global_pose.pose.position.x - robot_pose.pose.position.x;
    // double delta_y = global_pose.pose.position.y - robot_pose.pose.position.y;
    // double cos_yaw = std::cos(-yaw);
    // double sin_yaw = std::sin(-yaw);
    // local_pose.pose.position.x = cos_yaw * delta_x - sin_yaw * delta_y;
    // local_pose.pose.position.y = sin_yaw * delta_x + cos_yaw * delta_y;
    // double yaw_diff = math_utils::diffAngle(
    //     msg_convert::toYaw(robot_pose.pose.orientation),
    //     msg_convert::toYaw(global_pose.pose.orientation)
    // );
    // local_pose.pose.orientation = msg_convert::toGeometryQuaternion(yaw_diff);

    return local_pose; 
}

// 局部位姿转全局
geometry_msgs::PoseStamped ControllerBase::convertLocalPoseToGlobal(geometry_msgs::PoseStamped local_pose, 
    geometry_msgs::PoseStamped robot_pose){
    geometry_msgs::PoseStamped global_pose;

    // 方式一
    tf2::Transform local_pose_tf;
    tf2::fromMsg(local_pose.pose, local_pose_tf);
    tf2::Transform robot_pose_tf;
    tf2::fromMsg(robot_pose.pose, robot_pose_tf);

    // 计算全局位姿（局部位姿在机器人坐标系中的全局变换）
    tf2::Transform global_pose_tf = robot_pose_tf * local_pose_tf;
    tf2::toMsg(global_pose_tf, global_pose.pose);

    // 方式二
    // double yaw = tf2::getYaw(robot_pose.pose.orientation);
    // double delta_x = local_pose.pose.position.x;
    // double delta_y = local_pose.pose.position.y;
    // double cos_yaw = std::cos(yaw);
    // double sin_yaw = std::sin(yaw);
    // global_pose.pose.position.x = robot_pose.pose.position.x + cos_yaw * delta_x - sin_yaw * delta_y;
    // global_pose.pose.position.y = robot_pose.pose.position.y + sin_yaw * delta_x + cos_yaw * delta_y;
    // double yaw_add = math_utils::addAngle(
    //     msg_convert::toYaw(robot_pose.pose.orientation),
    //     msg_convert::toYaw(local_pose.pose.orientation)
    // );
    // global_pose.pose.orientation = msg_convert::toGeometryQuaternion(yaw_add);

    return global_pose;
}

// 全局路径转局部
vector<geometry_msgs::PoseStamped> ControllerBase::convertGlobalPathToLocal(vector<geometry_msgs::PoseStamped> global_path, 
    geometry_msgs::PoseStamped robot_pose){
    vector<geometry_msgs::PoseStamped> local_path;
    for(auto pose : global_path){
        local_path.push_back(convertGlobalPoseToLocal(pose, robot_pose));
    }
    return local_path;
}

// 局部路径转全局
vector<geometry_msgs::PoseStamped> ControllerBase::convertLocalPathToGlobal(vector<geometry_msgs::PoseStamped> local_path, 
    geometry_msgs::PoseStamped robot_pose){
    vector<geometry_msgs::PoseStamped> global_path;
    for(auto pose : local_path){
        global_path.push_back(convertLocalPoseToGlobal(pose, robot_pose));
    }
    return global_path;
}

/*************************************************
* 辅助函数
*************************************************/

// 旋转至目标角度
void ControllerBase::rotateToTGoal(geometry_msgs::PoseStamped robot_pose,
    geometry_msgs::PoseStamped goal_pose, geometry_msgs::Twist& cmd_vel){
    cmd_vel.linear.x = 0;
    cmd_vel.linear.y = 0;

    double angle_diff = math_utils::diffAngle(
        msg_convert::toYaw(robot_pose.pose.orientation),
        msg_convert::toYaw(goal_pose.pose.orientation)
    );
    // 最大减速 v^2 = v0^2 + 2as
    double max_speed_to_stop = maxSpeedToStop(baseConfig_.max_vel_yaw, angle_diff);
    max_speed_to_stop = angle_diff > 0 ? max_speed_to_stop : -max_speed_to_stop;

    // 最大加速限制 [__min_vel_limit ~ __max_vel_limit]
    cmd_vel.angular.z = min(baseConfig_.max_vel_yaw, max(baseConfig_.min_vel_yaw, max_speed_to_stop));
}

// 计算最大减速距离
double ControllerBase::maxSpeedToStop(double vel, double acc){
    // 最大减速 v^2 = v0^2 + 2as
    return sqrt(2 * vel * fabs(acc));
}

// 计算直线刹车距离 v^2 - 0 = 2as
double ControllerBase::maxStopDistance(geometry_msgs::Twist cmd_vel, double acc){
    return std::pow(cmd_vel.linear.x, 2) / (2 * acc);
}

// 检测目标点是否到达  距离判断
bool ControllerBase::isPositionReached(geometry_msgs::PoseStamped robot_pose, 
    geometry_msgs::PoseStamped goal_pose){
    double dist = math_utils::distanceBetweenPoses(goal_pose.pose, robot_pose.pose); 
    GLOG_INFO << "[isPositionReached] dist: " << dist << ", goal_distance_tolerance: " << baseConfig_.goal_distance_tolerance;
    if(fabs(dist) < baseConfig_.goal_distance_tolerance){
        return true;
    }
    return false;       
}


// 检测目标点是否到达  角度判断
bool ControllerBase::isAngleReached(geometry_msgs::PoseStamped robot_pose, 
    geometry_msgs::PoseStamped goal_pose){
    double theta = math_utils::diffAngle(
        msg_convert::toYaw(robot_pose.pose.orientation),
        msg_convert::toYaw(goal_pose.pose.orientation)
    );

    GLOG_INFO << "[isAngleReached] theta: " << theta << ", goal_angle_tolerance: " << baseConfig_.goal_angle_tolerance;
    if(fabs(theta) < baseConfig_.goal_angle_tolerance){
        return true;
    }
    return false;
}

// 检测目标点是否到达
bool ControllerBase::isGoalReached(geometry_msgs::PoseStamped robot_pose, geometry_msgs::PoseStamped goal_pose){
    return isPositionReached(robot_pose, goal_pose) && isAngleReached(robot_pose, goal_pose);
}


// 路径裁剪
vector<geometry_msgs::PoseStamped> ControllerBase::pruningLocalPath(vector<geometry_msgs::PoseStamped> local_path, 
    double limit_x, double limit_y){
    vector<geometry_msgs::PoseStamped> pruned_path;   

    // 确保路径非空
    if (local_path.empty()) {
        return pruned_path;
    }

    // 找到最近点
    size_t closest_index = -1;
    for (size_t i = 0; i < local_path.size(); ++i) {
        double dist = math_utils::distanceBetweenPoses(local_path[i].pose, localOriginPose_.pose);
        if (dist < 0.1) {
            closest_index = i;
            break;
        }
    } 

    // 如果找到前进方向最近点，则裁剪路径
    if(closest_index >= 0){
        for (size_t i = closest_index; i < local_path.size(); ++i) {
            const auto& path_point = local_path[i];
            double dx = path_point.pose.position.x - localOriginPose_.pose.position.x;
            double dy = path_point.pose.position.y - localOriginPose_.pose.position.y;
            if ((std::fabs(dx) > limit_x) || (std::fabs(dy) > limit_y)) {
                break;
            }
            pruned_path.push_back(local_path[i]);
        }
    } 
    
    return pruned_path;
}

// 发布局部路径
void ControllerBase::publishLocalPath(vector<geometry_msgs::PoseStamped> local_path){
    nav_msgs::Path path;
    path.header.stamp = ros::Time::now();
    path.header.frame_id = baseFrameId_;
    for(auto pose : local_path){
        geometry_msgs::PoseStamped p;
        p.header = path.header;
        p.pose = pose.pose;
        path.poses.push_back(p);
    }
    pathLocalPub_.publish(path);
}

// 获取前瞻距离
double ControllerBase::getLookAheadDistance(double vt){
  double lookahead_dist = fabs(vt) * baseConfig_.lookahead_time;
  return max(baseConfig_.min_lookahead_distance, 
    min(baseConfig_.max_lookahead_distance, lookahead_dist));
}

// 获取前瞻目标点
bool ControllerBase::getLookaheadGoal(vector<geometry_msgs::PoseStamped> local_path, 
    double lookahead_distance, geometry_msgs::PoseStamped& lookahead_goal){
    if(local_path.empty()){
        return false;
    }
    // //简单的距离计算，后期优化
    // geometry_msgs::PoseStamped origin_data;
    // for(size_t i = local_path.size()-1; i >=0 ; --i){
    //     double dis = math_utils::distanceBetweenPoses(local_path[i].pose, origin_data.pose);
    //     if(dis < lookahead_distance){
    //         lookahead_goal = local_path[i];
    //         return true;
    //     }
    // }
    
    geometry_msgs::PoseStamped origin_data;
    // Find the first pose which is at a distance greater than the lookahead distance
    auto goal_pose_it = std::find_if(local_path.begin(), local_path.end(), [&](const geometry_msgs::PoseStamped& ps) {
        return math_utils::distanceBetweenPoses(ps.pose, localOriginPose_.pose) >= lookahead_distance;
    });

    if(goal_pose_it == local_path.end()){
        goal_pose_it = std::prev(local_path.end());
        lookahead_goal = *goal_pose_it;
        return true;
    }
    else{
        geometry_msgs::PoseStamped goal_posestamp = *goal_pose_it;
        geometry_msgs::PoseStamped prev_posestamp;
        if (goal_pose_it == local_path.begin()){
            prev_posestamp = localOriginPose_;
        }
        else{
            auto prev_pose_it = std::prev(goal_pose_it);
            auto prev_posestamp = *prev_pose_it;
        }

        // 计算插值位姿  圆心为原点，半径为lookahead_distance
        std::pair<double, double> center_p(localOriginPose_.pose.position.x, localOriginPose_.pose.position.y);
        std::pair<double, double> prev_p(prev_posestamp.pose.position.x - localOriginPose_.pose.position.x, 
            prev_posestamp.pose.position.y - localOriginPose_.pose.position.y);
        std::pair<double, double> goal_p(goal_posestamp.pose.position.x - localOriginPose_.pose.position.x,
            goal_posestamp.pose.position.y - localOriginPose_.pose.position.y);
        vector<pair<double, double>> intersectionPoints = graphis_utils::circleLineIntersection(
            prev_p, goal_p, center_p, lookahead_distance
        );

        if(!intersectionPoints.empty()){
            lookahead_goal.pose.position.x = intersectionPoints[0].first;
            lookahead_goal.pose.position.y = intersectionPoints[0].second;
            double angle = math_utils::angleBetweenPoses(prev_posestamp.pose, goal_posestamp.pose);
            lookahead_goal.pose.orientation = msg_convert::toGeometryQuaternion(0.0, 0.0, angle);
            return true;
        }
    }
    return false;        
}

// 发布前瞻目标点
void ControllerBase::publishLookaheadGoal(geometry_msgs::PoseStamped lookahead_goal){
    lookahead_goal.header.stamp = ros::Time::now();
    lookahead_goal.header.frame_id = baseFrameId_;
    poseLookaheadPub_.publish(lookahead_goal);
}

/*************************************************
* 速度控制
*************************************************/

// 急停控制
void ControllerBase::e_stop(){
    sendEmptyTwist();
}

// 减速控制
void ControllerBase::slow_down(double controller_frequency){
    // v = v0 - at
    geometry_msgs::Twist cmd_vel;
    cmd_vel.linear.x = max(0.0, currCmdVel_.linear.x - (baseConfig_.linear_acc * controller_frequency));
    cmd_vel.linear.y = max(0.0, currCmdVel_.linear.y - (baseConfig_.linear_acc * controller_frequency));
    cmd_vel.angular.z = max(0.0, currCmdVel_.angular.z - (baseConfig_.angular_acc * controller_frequency));
    sendVelocity(cmd_vel);
}

// 停止控制
void ControllerBase::sendEmptyTwist(){
    geometry_msgs::Twist empty_twist;
    currCmdVel_ = empty_twist;          //当前速度数据缓存
    sendVelocity(empty_twist);
}

// 发送速度
void ControllerBase::sendVelocity(geometry_msgs::Twist& cmd_vel){
    // 需要添加限幅和平滑处理

    // 速度限制
    cmd_vel.linear.x = max(min(baseConfig_.max_vel_x, cmd_vel.linear.x), baseConfig_.min_vel_x);   
    cmd_vel.linear.y = 0.0;
    cmd_vel.angular.z = max(min(baseConfig_.max_vel_yaw, cmd_vel.angular.z), baseConfig_.min_vel_yaw);

    // cout << "[sendVelocity] cmd_vel: " << msg_convert::toString(cmd_vel) << endl;
    currCmdVel_ = cmd_vel;              //当前速度数据缓存
    speedPub_.publish(cmd_vel);
}
